#include "Quad9Element.h"

#include <cmath>

using SG::Algebra::Matrixd;
using SG::DataStructure::Common::Real;
using SG::FEMSolver::BeamElement::Quad9Element;

std::size_t Quad9Element::s_nNode = 9;

Quad9Element::Quad9Element () = default;

Quad9Element::~Quad9Element () = default;

void Quad9Element::Initialize ()
{
    const auto& integ = m_integForm.GetPoints ();
    m_shpFunc.Compute (integ);
}

void Quad9Element::CalculateGeomProp (const Matrixd& coord)
{
    // 初始化几何属性
    m_geomProp.Area = 0.0;
    m_geomProp.Qy   = 0.0;
    m_geomProp.Qz   = 0.0;
    m_geomProp.Iy   = 0.0;
    m_geomProp.Iz   = 0.0;
    m_geomProp.Iyz  = 0.0;

    Matrixd    jacoMat (2, 2);                        // 雅各比矩阵
    const auto nGauss = m_integForm.GetPointSize ();  // 获取积分点个数

    // 循环积分点计算面积和惯性特性
    for (int iGauss{ 0 }; iGauss < nGauss; ++iGauss)
    {
        const auto& DNDxi  = m_shpFunc.GetShapeFunctionDerivative (iGauss);                      // 形函数偏微分
        jacoMat            = DNDxi * coord;
        const Real detJaco = jacoMat (0, 0) * jacoMat (1, 1) - jacoMat (0, 1) * jacoMat (1, 0);  // 雅各比行列式
        const Real weight  = m_integForm.GetPoint (iGauss).m_weight;
        const Real dA      = detJaco * weight;

        // 计算面积
        m_geomProp.Area += dA;

        // 计算当前高斯点的物理坐标
        const auto&   N          = m_shpFunc.GetShapeFunction (iGauss);
        const Matrixd coordGauss = N * coord;
        const Real    y          = coordGauss (0, 0);
        const Real    z          = coordGauss (0, 1);

        // 累加惯性相关积分项
        m_geomProp.Qy += z * dA;
        m_geomProp.Qz += y * dA;
        m_geomProp.Iy += z * z * dA;   // ∫Z² dA
        m_geomProp.Iz += y * y * dA;   // ∫Y² dA
        m_geomProp.Iyz += z * y * dA;  // ∫YZ dA
    }
}

void Quad9Element::ComputeEleMatrix (const Matrixd&                coord,
                                     const std::array<Real,3>&     inertiaMoment,
                                     const Real                    mu,
                                     _OUT Matrixd&                 Ke,
                                     _OUT std::vector<Matrixd>&    FSet) const
{
    Ke = Matrixd (s_nNode, s_nNode, 0.0);
    FSet.clear ();

    Matrixd Pe (s_nNode, 1, 0.0);  // 翘曲函数的单元有限元方程的荷载项
    Matrixd Py (s_nNode, 1, 0.0);  // Vy荷载下应力函数的单元有限元方程的荷载项
    Matrixd Pz (s_nNode, 1, 0.0);  // Vz荷载下应力函数的单元有限元方程的荷载项

    Matrixd B (2, 9);
    Matrixd J (2, 2);
    Matrixd coordGauss (1, 2);

    const auto nGauss = m_integForm.GetPointSize ();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        auto& DNDxi = m_shpFunc.GetShapeFunctionDerivative (iGauss);
        J           = DNDxi * coord;
        B           = SG::Algebra::inverse2X2 (J) * DNDxi;
        Real j      = J (0, 0) * J (1, 1) - J (0, 1) * J (1, 0);  // 雅各比矩阵行列式
        Real wgt    = m_integForm.GetPoint (iGauss).m_weight;

        // 单元刚度矩阵
        Ke += B.Transpose () * B * (j * wgt);

        // Saint-Venant 扭转问题求解翘曲函数右端力单元列向量 Pe
        auto& N    = m_shpFunc.GetShapeFunction (iGauss);
        coordGauss = N * coord;
        Matrixd tmp (2, 1);
        tmp (0, 0) = coordGauss (0, 1);
        tmp (1, 0) = -coordGauss (0, 0);
        Pe += B.Transpose () * tmp * j * wgt;

        // y 方向横向剪切力 Vy 问题求解应力函数右端力单元列向量 Py
        Real r     = coordGauss (0, 0) * coordGauss (0, 0) - coordGauss (0, 1) * coordGauss (0, 1);
        Real q     = 2.0 * coordGauss (0, 0) * coordGauss (0, 1);
        tmp (0, 0) = inertiaMoment[0] * r - inertiaMoment[2] * q;
        tmp (1, 0) = inertiaMoment[2] * r + inertiaMoment[0] * q;
        // Py += ( N.Transpose () * ((inertiaMoment[0] * coordGauss (0, 0) - inertiaMoment[2] * coordGauss (0, 1)) * 2.0 * (1.0 + mu)) + B.Transpose () * (tmp * 0.5 *
        // mu) ) * j * wgt;
        Py += (N.Transpose () * ((inertiaMoment[0] * coordGauss (0, 0) - inertiaMoment[2] * coordGauss (0, 1)) * 2.0)) * j * wgt;  // 暂时考虑 mu=0，当考虑 mu 时应采用上面的注释行计算

        // z 方向横向剪切力 Vz 问题求解应力函数右端力单元列向量 Pz
        tmp (0, 0) = -inertiaMoment[2] * r + inertiaMoment[1] * q;
        tmp (1, 0) = -inertiaMoment[1] * r - inertiaMoment[2] * q;
        // Pz += ( N.Transpose () * ((inertiaMoment[1] * coordGauss (0, 1) - inertiaMoment[2] * coordGauss (0, 0)) * 2.0 * (1.0 + mu)) + B.Transpose () * (tmp * 0.5 *
        // mu ) ) * j * wgt;
        Pz += (N.Transpose () * ((inertiaMoment[1] * coordGauss (0, 1) - inertiaMoment[2] * coordGauss (0, 0)) * 2.0)) * j * wgt;  // 暂时考虑 mu=0，当考虑 mu 时应采用上面的注释行计算
    }
    FSet.emplace_back (Pe);
    FSet.emplace_back (Py);
    FSet.emplace_back (Pz);
}

SG::Algebra::Matrixd Quad9Element::ComputeShearCoeff (const SG::Algebra::Matrixd&           coord,
                                                      const std::array<Real,3>&             inertiaMoment,
                                                      const SG::DataStructure::Common::Real mu,
                                                      const SG::Algebra::Matrixd&           eleNodePsi,
                                                      const SG::Algebra::Matrixd&           eleNodePhi) const
{
    Matrixd    J (2, 2);
    Matrixd    coordGauss (1, 2);
    Matrixd    psiDerivative (2, 1), phiDerivative (2, 1), dYZ (2, 1, { 0.0 }), hYZ (2, 1, { 0.0 }), temp (2, 1), temp1 (2, 1);
    const auto nGauss = m_integForm.GetPointSize ();
    Real       alphaIntegralY{ 0.0 }, alphaIntegralZ{ 0.0 }, alphaIntegralYZ{ 0.0 };

    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        const auto& DNDxi = m_shpFunc.GetShapeFunctionDerivative (iGauss);
        J                 = DNDxi * coord;
        Real j            = J (0, 0) * J (1, 1) - J (0, 1) * J (1, 0);  // 雅各比矩阵行列式
        Real wgt          = m_integForm.GetPoint (iGauss).m_weight;

        // 计算Guass点坐标
        auto& N    = m_shpFunc.GetShapeFunction (iGauss);
        coordGauss = N * coord;
        // 计算dy dz hy hz
        // Real ySqMinusZSq = coordGauss(0, 0) * coordGauss(0, 0) - coordGauss(0, 1) * coordGauss(0, 1);   // 计算 y² - z²
        // Real yMulZ = coordGauss(0, 0) * coordGauss(0, 1);

        /* 目前暂时考虑 mu = 0, 后期考虑mu != 0 时取消下面的注释即可
        dYZ (0, 0) = mu * (inertiaMoment[0] * ySqMinusZSq / 2.0 - inertiaMoment[2] * yMulZ);
        dYZ (1, 0) = mu * (inertiaMoment[0] * yMulZ + inertiaMoment[2] * ySqMinusZSq / 2.0);
        hYZ (0, 0) = mu * (inertiaMoment[1] * yMulZ - inertiaMoment[2] * ySqMinusZSq / 2.0);
        hYZ (1, 0) = -mu * (inertiaMoment[2] * yMulZ + inertiaMoment[1] * ySqMinusZSq / 2.0);
         */

        // 计算应力函数偏导数
        const auto DNDx = SG::Algebra::inverse2X2 (J) * DNDxi;  // 将形函数对参考域导数转化为对物理域导数
        psiDerivative   = DNDx * eleNodePsi;
        phiDerivative   = DNDx * eleNodePhi;
        // 计算积分项结果
        // temp = psiDerivative - dYZ;
        temp = psiDerivative;
        alphaIntegralY += (temp (0, 0) * temp (0, 0) + temp (1, 0) * temp (1, 0)) * j * wgt;
        // temp1 = phiDerivative - hYZ;
        temp1 = phiDerivative;
        alphaIntegralZ += (temp1 (0, 0) * temp1 (0, 0) + temp1 (1, 0) * temp1 (1, 0)) * j * wgt;
        alphaIntegralYZ += (temp (0, 0) * temp1 (0, 0) + temp (1, 0) * temp1 (1, 0)) * j * wgt;
    }
    return Matrixd (3, 1, { alphaIntegralY, alphaIntegralZ, alphaIntegralYZ });
}

Matrixd Quad9Element::ComputeSectorProduct (const SG::Algebra::Matrixd& eleNodeCoord, const SG::Algebra::Matrixd& eleNodeOmega) const
{
    Matrixd    Iyzw (1, 2);
    const auto nGauss = m_integForm.GetPointSize ();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        auto  J = m_shpFunc.GetShapeFunctionDerivative (iGauss) * eleNodeCoord;
        auto  j = J (0, 0) * J (1, 1) - J (0, 1) * J (1, 0);
        auto& N = m_shpFunc.GetShapeFunction (iGauss);
        Iyzw += (N * eleNodeOmega) * (N * eleNodeCoord) * j * m_integForm.GetPoint (iGauss).m_weight;
    }
    return Iyzw;
}